#ifndef SAMPLE_TREE
#define SAMPLE_TREE

#include "Flann_wrapper.h"
#include "Point.h"
#include <vector>
#include <list>

class Sample_tree
{
public:

	//defult CTOR
	Sample_tree()
	{}

	Sample_tree(Point & source):
	  m_line(true),m_point(source)
	{
		add_vertex(source);
		m_point.set_index(0);
	}

	// given an id returns a point
	Point	get_point(int point_id)
	{
		return m_id_to_point[point_id];
	}

	/*	Adds a new vertex to the tree, updates its index and 
		adds the new vertex to the nearest-neighbor search structure. */
	void	add_vertex(Point p, bool add_to_nn = true)
	{
		p.set_index(m_id_to_point.size());
		m_id_to_point.push_back(p);
		m_parent_adjacency.push_back(-1);
	
		if (add_to_nn)
			m_nn_search.insert(p);
	}
	
	// Adds an edge between the two vertices
	void	add_edge(Point s, Point t)
	{
		m_parent_adjacency[get_id(t)] = get_id(s);
	}

	
	//Finds the nearest point to the sample
	Point	nearest(Point p)
	{
		return m_nn_search.nearest_neighbor(p);
	}

	/** 
	 * get_path(Point p,bool reverse = true) 
	 * recovers the path between the source point and Point p
	 * can reverse the path 
	 */
	list<Point>	get_path(Point p,bool reverse = true)
	{
		list<Point> result;

		result.push_back(p);

		while (get_id(result.back()) != get_id(m_point))
		{
			int current_id = get_id(result.back());
			int parent_id = m_parent_adjacency[current_id];
			result.push_back(m_id_to_point[parent_id]);
		}

		if(reverse)
		{
			result.reverse();
		}
		return result;
	}

	//does the line still in use
	bool need_line()
	{
		return m_line;
	}
	//the line is no longer in use
	void line_off()
	{
		m_line = false;
	}

	Point get_last_point()
	{
		return m_id_to_point.back();
	}

private:
	
	// given a point returns its id
	int	get_id(Point p)
	{
		return p.index();
	}

	Flann_wrapper	m_nn_search;		// nearest-neighbors search structure

	vector<int>		m_parent_adjacency; // for every vertex stores its parent in the tree

	vector<Point>	m_id_to_point;		// contain the id of every point in the path	

	bool m_line;						// if the robot using the line - didn't get out of its source yet

	Point m_point;						// the source point

};


#endif